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Abstract An efficient and accurate computational approach is proposed for optimal attitude con- 
trol of a rigid body. The problem is formulated directly as a discrete time optimization problem 



1 using a Lie group variational integrator. Discrete necessary conditions for optimality are derived, 
and an efficient computational approach is proposed to solve the resulting two point boundary 
value problem. The use of geometrically exact computations on SO (3) guarantees that this opti- 
^ '. mal control approach has excellent convergence properties even for highly nonlinear large angle 



attitude maneuvers. Numerical results are presented for attitude maneuvers of a 3D pendulum and 
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a spacecraft in a circular orbit. 
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1 Introduction 

A discrete optimal control problem for attitude dynamics of a rigid body in the presence of an 
attitude dependent potential is considered. The objective is to minimize the square of the l 2 norm 
of external control torques which transfer a given initial attitude and an initial angular momentum 
of the rigid body to a desired terminal attitude and a terminal angular momentum during a fixed 
maneuver time. The attitude of the rigid body is defined by the orientation of a body fixed frame 
with respect to a reference frame; the attitude is represented by a rotation matrix that is a 3 x 3 
orthogonal matrix with determinant of 1 . Rotation matrices have a Lie group structure denoted by 
SO(3). 

The dynamics of a rigid body has fundamental invariance properties. In the absence of noncon- 
servative forces, total energy is preserved. A consequence of Noether's theorem is that symmetries 
in the Lagrangian result in conservation of the associated momentum map. Furthermore, the con- 
figuration space of the rigid body has the orthogonal structure of the Lie group SO (3). General- 
purpose numerical integration methods, including the popular Runge-Kutta schemes, typically 
preserve neither first integrals nor the geometric characteristics of the configuration space. In par- 
ticular, the orthogonal structure of the rotation matrices is not preserved numerically with standard 
schemes. 

A Lie group variational integrator that preserves those geometric features is presented in [1 J, and 
integrators on the configuration space SO(3) and SE(3) are developed in [2| and [3|, respectively. 
These integrators are obtained from a discrete variational principle, and they exhibit the character- 
istic symplectic and momentum preservation properties, and good energy behavior characteristic 
of variational integrators [4J. Since the rotation matrices are updated by a group operation, they 
automatically evolve on the rotation group without the need for reprojection techniques or con- 
straints 0. 

Optimal attitude control problems are studied in [6|. The angular velocity of a rigid body is 
treated as a control input; an optimal angular velocity that steers the rigid body is derived from 
the attitude kinematics. Continuous time optimal control problems on a Riemannian manifold are 
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studied in 0, where necessary conditions for optimality are derived from a variational principle. 
An optimal control problem based on discrete mechanics is studied in [ 8 1 . The discrete equations of 
motion and the boundary conditions are imposed as constraints, and the optimal control problem is 
solved by a general-purpose parameter optimization tool. This approach requires large computation 
time, since the number of optimization parameters is proportional to the number of discrete time 
steps. Discrete necessary conditions for optimal control of the attitude dynamics of a rigid body 
are presented in J9|. 

This paper proposes an exact and efficient computational approach to solve an optimal control 
problem associated with the attitude dynamics of a rigid body that evolves on the configuration 
space SO (3). We assume that the control inputs are parameterized by their value at each time step. 
A Lie group variational integrator on SO (3) that includes the effects of external control inputs 
is developed using the discrete Lagrange-d'Alembert principle. Discrete necessary conditions for 
optimality are obtained using a variational principle, while imposing the Lie group variational 
integrator as dynamic constraints. 

The necessary conditions are expressed as a two point boundary value problem on T*SO(3) and 
its dual. Sensitivity derivatives along an extremal solution are developed by following the proce- 
dures presented in ifTOl . and they are used to construct an algorithm that solves the boundary value 
problem efficiently. Since the attitude of the rigid body is represented by a rotation matrix, and the 
orthogonal structure of rotation matrices is preserved by the Lie group variational integrator, the 
discretization of the optimal control problem does not exhibit singularities. 

This paper is organized as follows. In Sectional a Lie group variational integrator is devel- 
oped using the discrete Lagrange-d'Alembert principle. Necessary conditions for optimality and a 
proposed approach to solve the two point boundary problem are presented in Section |3 Numerical 
results for the attitude control of an underactuated 3D pendulum, and for a fully actuated spacecraft 
in a circular orbit are given in Section |U 
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2 Equations of Motion for the Attitude Dynamics of a Rigid Body 

In this section, we define a rigid body model in a potential field and we develop discrete equations 
of motion for the attitude dynamics of the rigid body, referred to as a Lie group variational inte- 
grator. These discrete equations of motion are used as dynamic constraints for the optimal control 
problem presented in Section |3j 

2.1 Rigid body model 

We consider the attitude dynamics of a rigid body in the presence of an attitude dependent potential. 
The configuration space is the Lie group, SO(3). We assume that the potential U (•) : SO(3) t— > K is 
determined by the attitude of the rigid body, R E SO (3). External control inputs generate moments 
about the mass center of the rigid body. A spacecraft on a circular orbit including gravity gradient 
effects ifTOl . a 3D pendulum [3, or a free rigid body can be modeled in this way. The continuous 
equations of motion are given by 

tl+Qxn = M + Bu, (1) 
R = RS(f2), (2) 

where Q E M 3 is the angular velocity of the body expressed in the body fixed frame, and II = 
J J? E M 3 is the angular momentum of the body for a moment of inertia matrix J E R 3x3 . Mel 3 
is the moment due to the potential, u E M m is the external control input, and B E M 3xm is an 
input matrix. If the rank of the input matrix is less than 3, then the rigid body is underactuated. The 
matrix valued function, £>(•) : IR 3 i— ► so(3) is a skew mapping defined such that S(x)y = x x y 
for all x,y E M 3 . The Lie algebra so (3) is identified by 3 x 3 The moment due to the potential is 
determined by the relationships, S(M) = ^ R — R T j^, or more explicitly, 

M = r\ x v n + r 2 x v T2 +r 3 x v ri , (3) 

where ri, v n E M lx3 are the ith row vectors of R and respectively. A detailed derivation of the 
above equations can be found in [2]. 
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2.2 Lie group variational integrator 

The attitude dynamics of a rigid body exhibit geometric invariant features. In the absence of an 
external control input, the total energy is preserved. If there is a symmetry in the potential func- 
tion, the corresponding momentum map is preserved. The attitude as described by a rotation matrix 
is always orthogonal. Classical numerical integration methods typically preserve neither first in- 
tegrals nor the geometry of the configuration space, SO (3). In particular, standard Runge-Kutta 
method fail to capture the energy dissipation of a controlled system accurately E). 

It is often proposed to parameterize © by Euler angles or quaternions instead of integrating © 
directly. However, Euler angles have singularities, and the unit length of a quaternion vector is not 
preserved by classical numerical integration. Furthermore, renormalizing the quaternion vector at 
each step tends to break other conservation properties. 

We describe a Lie group variational integrator that respects these geometric properties. It is 
obtained from a discrete variational approach, and therefore it exactly preserves the momentum 
and symplectic form, while exhibiting good energy behavior over exponentially long times. Since 
a Lie group numerical method is explicitly adopted, the rotation matrix automatically remains 
onSO(3). 

The Lie group variational integrator is obtained by following procedures commonly adopted 
in Lagrangian mechanics. The variational approach is based on discretizing Hamilton's principle 
rather than discretizing the continuous equations of motion. The velocity phase space of the con- 
tinuous Lagrangian is replaced by discrete variables, and a discrete Lagrangian is chosen. Taking 
a variation of the action sum defined as the summation of the discrete Lagrangian, we obtain a 
Lagrangian form of the discrete equations of motion using the Lagrange-d'Alembert principle. A 
discrete version of the Legendre transformation yields a Hamiltonian form. 

The detailed derivation is presented in Q and Q. In this paper, we extend these results to 
include the effects of external control inputs. Consider the fixed integration step size /iGl. Let 
Rk £ SO (3) denote the attitude of the rigid body at time t = kh. We introduce a new variable 



6 



F k E SO (3) defined by 

F k = R^Rk+i, (4) 

which represents a relative attitude between integration steps. If we find F k E S0(3) at each 
integration step, the rotation matrix is updated by multiplication of two rotation matrices, i.e. 
Rk+i = RkFk, which is a group operation on SO(3). This guarantees that the rotation matrix 
evolves on SO(3) automatically. This is the approach of Lie group methods Q. The following 
procedure provides an expression for F k using the discrete Lagrange-d'Alembert principle. 
Using the kinematic relationship ©, S(f2 k ) can be approximated as 

S(f2 k ) = R k R k » R\ =\{F k - J 3X3 ) • 

Using the above equation, we can show that the kinetic energy of the rigid body is given by 

T = l -tx[S{n k )J d S{n k ) T ] = ^tr[(I 3x3 - F k ) J d ] , 

where Jd E IR 3x3 is a nonstandard moment of inertia matrix of the rigid body defined in terms 
of the standard moment of inertia matrix J E R 3x3 as Jd = |tf J] /3X3 — J- Define a discrete 
Lagrangian L d as 

L d {R k , F k ) = itr[(/ 3x3 - F k ) J d \ - W{R k+l ). (5) 
h 

This discrete Lagrangian is a first-order approximation of the integral of the continuous Lagrangian 
over one integration step. Therefore, the following action sum, defined as the summation of the dis- 
crete Lagrangian, approximates the action integral; <3 d = J2k=o Ld(Rk, F k ). Taking a variation of 
the action sum, we obtain the discrete equations of motion using the discrete Lagrange-d'Alembert 
principle. The variation of a rotation matrix can be expressed using the exponential of a Lie algebra 
element: 



R% — Rk£ e 
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where e G R and % e 50 (3) is the variation expressed as a skew symmetric matrix. Thus the 
infinitesimal variation is given by 

d 



8R k = -r 

ae 



R% = RkVk- (6) 

£=0 



The Lagrange-d' Alembert principle states that the following equation is satisfied for all possible 
variations r\ k 6 so (3). 

6 ^( J 3x 3 - F k ) J d ] - hU(R k+1 ) - -tiVk + iS(Bu k+1 )] = 0. (7) 

fc=0 k=0 

Using the expression of the infinitesimal variation of a rotation matrix © and using the fact that 
the variations vanish at the end points, the above equation can be written as 



N ~ l r n du h 

Vk I £ {F k Jd - J d F k -i) + hRlg^ - ^S{Bu k ) 



k=l 



0. 



Since the above expression should be zero for all possible variations rjk £ so(3), the expression 
in the braces should be symmetric. Then, the discrete equations of motion in Lagrangian form are 
given by 

i (F k+1 J d - J d F k - J d Fl +1 + if Jj = hS(M k+1 ) + hS(Bu k+1 ), (8) 

Rk+l = RkF k . (9) 

Using the discrete version of the Legendre transformation, the discrete equations of motion in 
Hamiltonian form are given by 

hS(n k ) = F k J d - J d Fl, (10) 
Rk+i = R k F k , (11) 
I7 k+1 = F k II k + h (Affc+i + Bu k+l ) . (12) 

Given (R k , II k ), we can obtain F k by solving (flOb . and R k +i is obtained by (fTTT) . The moment 
due to the potential M^+i can be calculated by ©. Finally, n k+ i is updated by (fT2l . This yields a 
map (i? fc , i7 fc ) 1 — ^ (-Rfc+i, n k+1 ), and this process can be repeated. The only implicit part is solving 
(ITOb . We can express (fTOb in terms of a Lie algebra element S(f k ) = logm(Ffc) e so(3), and find 
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f k E R 3 numerically by a Newton iteration. The relative attitude F k is obtained by the exponential 
map: F k = e s ^ k \ Therefore we are guaranteed that F k is a rotation matrix. 

The order of the variational integrator is equal to the order of the corresponding discrete La- 
grangian. Consequently, the above Lie group variational integrator is of first order since © is a 
first-order approximation. While higher-order variational integrators can be obtained by modifying 
(O, we use the first-order integrator because it yields a compact form for the necessary conditions 
that preserves the geometry; these necessary conditions are developed in SectionOJ 

3 Discrete Optimal Control of the Attitude Dynamics of a Rigid Body 

We formulate a discrete optimal control problem for the attitude dynamics of a rigid body, and we 
derive necessary conditions for optimality using a variational principle. The necessary conditions 
are expressed as a two point boundary value problem, and a computational approach to solve the 
boundary value problem is proposed using sensitivity derivatives. 

3.1 Problem formulation 

A discrete time optimal control problem in SO (3) is formulated as a maneuver of a rigid body from 
a given initial attitude R E SO (3) and an initial angular momentum i7 E R 3 to a desired terminal 
attitude R% E SO (3) and a terminal angular momentum II fj E R 3 during a given maneuver time 
N. The performance index is the square of the l 2 norm of the control input; the discrete equations 
of motion developed in the previous section are imposed as constraints. 



given: R ,n , R d N ,n d Nl N, 




N-l 



such that R 



11 M 



JV) 



subject to (TTOl), {jj} and (fT2h . 
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In [8 1, an optimal control problem based on discrete mechanics is considered. The control in- 
puts at each discrete step are considered as optimization parameters, and the discrete equations 
of motion and the boundary conditions are imposed as constraints. The optimization problem is 
solved numerically by a general-purpose parameter optimization tool such as Sequential Quadratic 
Programming (SQP). The same approach can be applied to the above optimization problem. How- 
ever, it has a large computational burden since the number of optimization parameters, m x N, is 
proportional to the number of integration steps. Usually, a large time step size is chosen to make 
the number of integration steps small, or the control inputs are approximated by collocation points. 
The resulting control inputs tends to be under-resolved and sub-optimal. 

We derive necessary conditions for optimality using the standard calculus of variations. We 
assume that the control inputs are parameterized by their value at each time step. The necessary 
conditions are expressed as a two point boundary value problem. 

3.2 Necessary conditions of optimality 
Define an augmented performance index as 



where X k , \ 2 k G M 3 , are Lagrange multipliers corresponding to the discrete equations of motion. 
The augmented performance index is chosen such that the dimension of the multipliers is equal to 
the dimension of the rotation matrix and the angular momentum vector. The discrete kinematics 
equation (fTTTl is transformed into a matrix logarithm form. The constraints arising from the discrete 
kinematics equation dTTT) and the angular momentum equation d 1 2b are explicitly applied. Equation 
dTUt appears in the discrete equations of motion because we introduce the auxiliary variable F k e 
SO (3). The constraint (TTOb is considered implicitly when taking a variation of the performance 
index. 




k=0 



+ x 2 k T {-n k+1 + Fln k + h (M k+1 + Bu k+1 )} 



(13) 
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Consider small variations from a given trajectory denoted by FI kl R k , F k , u k : 

n e k = n k + esn k , (14) 

Rl = R k e^\ 

= R k + eR k S(( k ) + 0(e 2 ), (15) 

= F k + eF k S(£ k ) + 0(e 2 ) } (16) 

where 6s £ R 3 ~ so (3). The real space IR 3 is isomorphic to the Lie algebra so (3) according to 
the skew mapping S(-) : R 3 i— > so (3). The variations of the rotation matrices are expressed using 
the exponential of the Lie algebra elements. The corresponding infinitesimal variations of II k , R k , 
and F k are given by SII k , 5R k = R k S(( k ), and SF k = F k S(^ k ), respectively. 

The variation of the augmented performance index is obtained from the above expressions. 
Instead of taking a variation of the matrix logarithm in (fT3t . we take a variation of the kinematics 
equation, (ITU and we use it as a constrained variation. Since F k = R%R k+ i by (fTTT) . the variation 
5F k is given by 

SF k = 5R k R k+ i + R k SR k+ i. 
Substituting the expression for the infinitesimal variation of R k , we obtain 

F k S(£ k ) = S(C k )F k + F k S(( k+1 ). 

Multiplying both sides of the above equation by F^ and using the property S(R T x) = R T S(x)R 
for all R e SO (3) and x e IR 3 , we obtain 

^ = -FlC k + ( k+1 . (17) 

We use (ITvb as a constrained variation equivalent to (fTTT) . 

Now we develop another expression for a constrained variation using (ITOb . Since we do not use 
(ITDT) explicitly as a constraint in (fT3l) . 5II k and SF k are not independent. Taking a variation of (flOl) . 
we obtain 

hS(8II k ) = F k S(£ k )J d + J d S(£ k )Fl. 
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Using the properties, S{Rx) = RS{x)R T and S(x)A + A T S{x) = 5({tr[A] J 3x3 - A} x) for all 
x G R 3 , A G R 3x3 , and R G S0(3), the above equation can be written as 

hS{Sn k ) = S(F k £ k )F k J d + J d FZS(F k £ k ), 
= S({tr[F k J d }I 3x3 -F k J d }F k ^ k ). 

Thus, £ fc is given by 

Z k = B k 5n kl (18) 

where B k = hF k {tr[F k J d ] J 3x3 — F k J^} 1 G R 3x3 . Equation (fT8l shows the relationship between 
8II k and 8F k . 

Since the moment due to the potential M k is dependent on the attitude of the rigid body, the 
variation of the moment 5M k can be written using a variation of the rotation matrix: 

5M k = M k ( k , (19) 

where M. k g R 3x3 is expressed in terms of the the attitude of the rigid body, and the expression 
is determined by the potential field. We present detailed expressions of M. k in Section|4|for a 3D 
pendulum and for a spacecraft in a circular orbit. Using ([T71) and ( fTBl . 5M k+ i is given by 

SM k+1 = M k+ i( k +i, 

= M k+1 F£( k + M k+1 B k 5n k . (20) 

Now, we take a variation of the augmented performance index (fT3t using the constrained vari- 
ations (fT71) . (fTSl) . and (|2"0"1) . Using ( fTTl . the variation of the performance index is given by 

N-l 

$Ja = hSu k+l U k+l + ^ k T {Ck + F£( k - Cfc+l} 

fc=0 

+ \l T { -5n k+1 + 5F^n k + F^5n k + h5M k+1 + hB5u k+1 } . (21) 
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Substituting (l20b into (I2TT) and rearranging, we obtain 



JV-1 



SJ a = £ w «w-i K+i + ^} - & X AJ + CJ {^Aj + W^M^i**} 



fc=0 



- 5n T k+1 X\ + {F fc A^ + hB T k M T k+1 \l} 
+ £{-S(F£lI k )\l + \l}. 



(22) 



Substituting (fT8t into d22b and using the fact that the variations 577^ vanish at A; = 0, iV, we 
obtain 



JV-l 



SJ a = h5u l K + bTx I-i} + CJ {-ALi + F k Xl + hF k M T k+1 X 2 k } 

k=l 

+ Sn T k + (F k - B T k S{F^n k ) + hB T k M T k+l )Xl + B T k \\) . (23) 

Since 5J a = for all variations of 5u k , Cfc, ^77*. which are independent, the expression in the 
braces are zero. Thus we obtain necessary conditions for optimality as follows. 



n k+1 = Fln k + h (Mfc+i + Bu k+1 ) , 
hs(n k ) = F k J d - J d Fl, 

Rk+l = RkF k , 

u k+ i = —B X 2 k , 













\1 

A k+l 


A 




_°k+l 


u k+l_ 




_ A k+l_ 



(24) 
(25) 
(26) 
(27) 

(28) 



where 



Ah - F k , 

B k = hFl MF k J d ] I 3x3 - F k J d y l , 
C k = hM k+l F^ 

V k = if + S(F^n k )B k + hM k+1 B k . 



(29) 
(30) 
(3D 
(32) 



In the above equations, the only implicit part is (l25l) . For a given initial condition (Rq, i7 , Aq, Aq), 
we can find F by solving d23t . Then, Ri is obtained from (|2"6T) . Since m x = — Aq by (|2"71) . and Mi 
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is a function of R\, TI\ can be obtained using (|24b . We solve (l25l) to obtain F x using i7i. Finally, 
X\, \\ are obtained from (|2"%1 ). since A\,B\, C\,T>\ are functions of i?x, i7 l5 F x . This yields a map 
(i?o, -^o, Aq, Aq) i— > i7i, X\, Xl), and this process can be repeated. 

3.3 Two point boundary value problem 

The necessary conditions for optimality are given as a 12 dimensional two point boundary value 
problem on T*SO(3) and its dual space. This problem is to find 

Attitude and Angular momentum :Rk,IIk, 
Multiplier variables :Xl, X 2 k , 
Control Inputs 

for k = {0, 1, • • ■ , N}, to satisfy simultaneously, 

Equations of motion : jSJ) , (j25} , (O , 

Multiplier equations : (|28|) , 
Optimality condition : (J27|) , 
Boundary conditions :R , 77 , Rn, n N . 

An iterative numerical method for the two point boundary value problem is presented. A nominal 
solution that satisfies some of the above conditions is chosen, and this nominal solution is up- 
dated by successive linearization so that the remaining conditions are also satisfied as the process 
converges. 

We use a neighboring extremal method ifPTl . A nominal solution satisfies all of the necessary 
conditions except the boundary conditions. The neighboring extremal method is characterized as 
an iterative algorithm for improving estimates of the unspecified multiplier initial conditions so as 
to satisfy the specified terminal boundary conditions in the limit. This is sometimes referred to as a 
shooting method. The optimality condition (l2Vb is substituted into the equations of motion and the 
multiplier equations. The sensitivities of the specified terminal boundary conditions with respect to 



14 



the unspecified initial multiplier conditions can be calculated by direct numerical differentiation, or 
they can be obtained by a linear analysis. The main advantage of the neighboring extremal method 
is that the number of iteration variables is small. It is equal to the dimension of the equations of 
motion. The difficulty is that the extremal solutions are sensitive to small changes in the unspecified 
initial multiplier values. Therefore, it is important to compute the sensitivities accurately. 

We use linear analysis to compute the sensitivities. The sensitivity model is defined at the Lie 
algebra level as presented in ifTOll . It is natural to define the sensitivity model in the Lie algebra, 
since the Lie algebra is a linear vector space. The resulting sensitivity model is global, and it has 
the same dimension as the Lie group. The sensitivity derivatives in the Lie algebra are related to 
the original Lie group by the exponential map. 

Using the perturbation models defined in (TBI) . (fT5l) . the linearized equations of motion for the 
attitude dynamics can be written as 



Cfc+1 




A k 


B k 








3 X3 


sn k+1 




Ck 






. 6n K 




hBB T 



(33) 



Note that the homogeneous part of d33t is equivalent to equations that are the dual of d28t . The 
variation of the equations of motion is equivalent to the dual of the multiplier equations, so the 
variation of the multiplier equations is equivalent to the second variation of the attitude dynamics 
equations. The linearized equations of motion for the multipliers can be obtained as 



(^fe+l) Al x +1 A 



4 1 



Ck 

sn k 



+ (A 1 k 1 +i y T (hxs-A? +1 A?) 



8% 



(34) 



where A[ J 6 IR 6x6 



i, j G {1,2} are defined in the Appendix. 
Using d33l) and d34l) . the sensitivity derivatives of the attitude, angular momentum, and the 
multipliers can be written as 



Xk+l 


= A k 










5X k 



(35) 
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where x k = 8 II] 
given by 



T\T 



5\ k = [5Xl' T ,5Xl' T } T e R 6 , and A k e 



>12xl2 



. The solution of (l35t is 



5X N 



(N-i \ 

\k=0 J 


I 


X 

_5X _ 






x 


$21 $22 




SX 



(36) 



For the given two point boundary value problem x = since the initial attitude and the initial 
angular momentum are given, and Xn is free. Then, we obtain 



xn = $uSX c 



(37) 



The unspecified initial multipliers are A , and the specified terminal boundary conditions are the 
terminal attitude R d N and the terminal angular momentum II N , Thus <Pi 2 represents the sensitivity 
of the specified terminal boundary conditions with respect to the unspecified initial multipliers. 
Using this sensitivity, an initial guess of the unspecified initial conditions is iterated to satisfy the 
specified terminal conditions in the limit. 

Any type of Newton iteration can be applied to this problem using the sensitivity derivative as 
a gradient. The procedure uses a line search with backtracking algorithm, referred to as Newton- 
Armijo iteration in lfT2ll . The procedure is summarized in Table[H where i is the iteration index, and 
es, ol E R are a stopping criterion and a scaling factor, respectively. The outer loop finds a search 
direction by computing the sensitivity derivatives, and the inner loop performs a line search along 
the obtained direction; the error in satisfaction of the terminal boundary condition is determined 
on each iteration. 



4 Numerical Computations 

Numerical results are given for two optimal attitude control problems; optimal attitude control of 
an underactuated 3D pendulum and optimal attitude control of a fully actuated spacecraft on a 
circular orbit. 
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4.1 3D Pendulum 

A 3D pendulum is a rigid body supported at a frictionless pivot acting under the influence of 
uniform gravity |[T3l . The gravity potential, acting in the vertical or e 3 direction, is given by 

U(R) = -mge^Rp, (38) 

where m E R is the mass of the pendulum, g E R is the gravitational acceleration, and p E M 3 
represents a vector from the pivot point to the mass center of the pendulum in the body fixed frame. 
The pendulum model is shown in Fig.[TJ(a) with the pivot located at the origin, and we assume that 
the pendulum is axially symmetric. The gravity moment and its variations are given by 

M = mgp x R T e 3 , 
5M = M( = mgS(p)S(R T e 3 )(. 

There are two equilibrium manifolds; a hanging equilibrium manifold when Rp = \\p\\ e 3 , an 
inverted equilibrium manifold when Rp = — \\p\\ e 3 . 

The properties of the axially symmetric pendulum are given by J = diag [0.156, 0.156, 0.3] kg m 2 , 
m = 1kg, and p = [0, 0, |1 m. We assume that the component of the control input along the axis 
of symmetry is zero; this corresponds to an underactuated 3D pendulum. The corresponding input 
matrix is given by 

""l 

B= 01 


Two types of boundary conditions are considered. The first maneuver is to transfer the 3D pen- 
dulum from a hanging equilibrium to an inverted equilibrium. The second maneuver is a 180 degree 
rotation about the uncontrolled axis of symmetry starting in a hanging equilibrium. The terminal 
attitude also lies in the hanging equilibrium manifold. Each maneuver is completed in 1 sec. The 
time step size is h = 0.001 sec and the number of integration steps is N = 1000. The corresponding 
boundary conditions are given by 
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(i) Rotation from a hanging equilibrium to an inverted equilibrium. 

1 
10 , 
00-1 

-^o = O3X1, nfj = 03xi. 

(ii) Rotation from one hanging equilibrium to another hanging equilibrium. 

#o = / 3 x 3 , < = diag[-l,-l,l], 

no = 03x1, nfj = 03xi. 

The optimized performance index and the violation of the constraints are given in Table |2l 
The terminal boundary conditions are satisfied at the level of machine precision for both cases. 
Figures |2]and|3]show snapshots of the attitude maneuvers, the control input history, and the angular 
velocity response. (Simple animations which show these optimal attitude maneuvers of the 3D 
pendulum can be found at |http : / / www . umich . edu/ ~tylee| ) The third component of the 
angular velocity is constant; this is a conservation property of the controlled axially symmetric 3D 
pendulum. 

The optimized attitude maneuver of the first case is an eigen-axis rotation about the fixed axis; 
[ 2 ' 2 ' 0]- I n m e second case, the rotation about the axis of symmetry is induced from control 
moments about the first and second body fixed axes. The resulting attitude maneuver is more 
complicated, and it requires larger control inputs. 

Figures [21(d) and[3](d) show the violation of the terminal boundary condition according to the 
number of iterations in a logarithm scale. The circles denote outer iterations to compute the sensi- 
tivity derivatives. For all cases, the initial guesses of the unspecified initial multiplier are arbitrarily 
chosen such that the initial trial of control inputs is close to zero throughout the maneuver time. 
The error in satisfaction of the terminal boundary condition of the first case converges quickly to 
machine precision; only 7 iterations are required. A longer number of iterations is required in the 
second case, but the error converges exponentially to machine precision after the solution is close to 



Ro — -^3x3; Rn — 
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the local minimum at the 55th iteration. These convergence rates show the quadratic convergence 
property of Newton iteration. The convergence rates are dependent on the numerical accuracy of 
the sensitivity derivatives. 

4.2 Spacecraft on a Circular Orbit 

We consider a spacecraft on a circular orbit about a large central body, including gravity gradient 
effects ifbH . The spacecraft model is shown at Fig. [11(b). The attitude of the spacecraft is repre- 
sented with respect to the local vertical local horizontal (LVLH) axes. The gravity potential is given 



where G G R is the gravitational constant, M G R is the mass of the central body, r G R is the 



orbital radius, and lu = w^ir G R is the orbital angular velocity. The gravity moment and its 

V r o 

variations are given by 



There are 24 distinct relative equilibria for which the principal axes are exactly aligned with the 
LVLH axes, and the spacecraft angular velocity is identical to the orbital angular velocity of the 
LVLH coordinate frame. 

We assume that the spacecraft is fully actuated. The corresponding input matrix is B — I 3 x 3 - 
The mass, length and time dimensions are normalized by the mass of the spacecraft, a size scale 
factor of the spacecraft, and the orbital angular velocity lu q G R, respectively. The mass property 
of the spacecraft is chosen as J = diag [1, 2.8, 2]. 

Two boundary conditions are considered. Each maneuver is a large attitude change completed 
in a quarter of the orbit, 2/ = |. The time step size is h = 0.001 and the number of integration 
step is N = 1571. The terminal angular momentum is chosen such that the terminal attitude is 
maintained after the maneuver. 



by 



U(R) 



GM 



- -loI (tr[ J] - 3ejRJR T e 3 ) , 




M = 3uj%R T e 3 x JR T e 3 , 
§M = MC = Stol \ - S(JR T e 3 )S(R T e 3 ) + S (R T e 3 ) J S (R T e 3 )] ( 
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(iii) Rotation maneuver about the LVLH axis e\. 



< = diag[l,-l, 



1] 



n — ujqJRq62, nf, — uoJR d j^e2- 



(iv) Rotation maneuver about the LVLH axes e\ and e 2 : 



-10 



R = diag [1 



1 5 — 1] ; 



0-1 



0-10 



no = loqJRq e 2 , n N = u JR^ e 2 . 



The optimized performance index and the violation of the constraints are given in Table El Figures 
|4] and 13 show the attitude maneuver of the spacecraft (clockwise direction), the control inputs, the 
angular velocity response, and the violation of the terminal boundary condition according to the 
number of iterations. 

4.3 Numerical properties 

The neighboring extremal method or the shooting method are numerically efficient in the sense 
that the number of optimization parameters is minimized. But, this approach may tend to have 
numerical ill-conditioning lfT5l . A small change in the initial multiplier can cause highly nonlinear 
behavior of the terminal attitude and angular momentum. It is difficult to compute the Jacobian 
matrix for Newton iterations accurately, and consequently, the numerical error may not converge. 

However, the numerical examples presented in this section show excellent numerical conver- 
gence properties. They exhibit a quadratic rate of convergence. This is because the proposed com- 
putational algorithms on SO (3) are geometrically exact and numerically accurate. The attitude 
dynamics of a rigid body arises from Hamiltonian mechanics, which have neutral stability. The 
adjoint system is also neutrally stable. The proposed Lie group variational integrators and the dis- 
crete multiplier equations, obtained from variations expressed in the Lie algebra, can preserve the 
neutrally stability property. Therefore the sensitivity derivatives are computed accurately. 
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5 Conclusions 

A discrete optimal control problem for the attitude dynamics of a rigid body in the presence of an 
attitude dependent potential is studied. The performance index is the l 2 norm of external control 
inputs and boundary conditions on the attitude and the angular momentum are prescribed. The 
attitude is represented by a rotation matrix in the Lie group, SO (3). This paper proposes three 
levels of geometrically exact computations on SO (3) to solve the optimal control problem; Lie 
group variational integrator, discrete-time necessary conditions for optimality, and discrete-time 
sensitivity derivatives. 

The Lie group variational integrator obtained from a discrete variational principle preserves the 
geometric features of the attitude dynamics of the rigid body. It exhibits symplectic and momentum 
preservation properties, and good energy behavior characteristic of variational integrators. Since 
the rotation matrices are updated by a group operation, the Lie group structure is also preserved. 

The necessary conditions of optimality are derived by a variational principle. The Lie group 
variational integrators are imposed as constraints, and the variation of the rotation matrices are 
expressed in terms of Lie algebra elements. The proposed discrete optimality conditions are the 
basis for a numerically efficient computational algorithms for the optimal attitude control problem, 
since the implicit part of the optimality conditions occurs in a single equation of one variable. This 
implicit condition can be solved easily by Netwon iteration. Other algorithms require iteration on 
the entire discrete time trajectory simultaneously. 

The necessary conditions are expressed as a two point boundary value problem on T*SO(3) 
and its dual space. The sensitivity derivatives are developed in the Lie algebra, and the two point 
boundary value problem is solved using a neighboring extremal method. The neighboring extremal 
method is efficient for this class of optimal control problems because the resulting problem of satis- 
fying the terminal boundary conditions has a small number of variables. The main disadvantage is 
that a small change in the initial multipliers can produce a very large change in the terminal condi- 
tion. This can result in numerical ill-conditioning. The nonlinearity also makes it hard to construct 
an accurate estimate of the Jacobian matrix that is needed for a Newton iteration. In this paper, 
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the two point boundary problem is solved efficiently. The error in the terminal boundary condi- 
tions converges exponentially to machine precision. This is because the sensitivity derivatives are 
computed accurately in the Lie algebra of SO (3). 

Numerical results for an optimal attitude control problem involving an underactuated axially 
symmetric 3D pendulum and for an optimal attitude control problem involving a fully actuated 
spacecraft on a circular orbit are given. The boundary conditions are chosen such that the resulting 
maneuvers are large angle attitude maneuver. It is shown that the proposed numerical computations 
on SO (3) are geometrically exact and highly efficient. 
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Appendix 

The linearized equations of motion for the multiplier equations (l28t are given by 







^k+1 ^k+1 




K+i 


+ 


•^■k+l 


























SXI 








_ A fc+i_ 




fi T 


u k+l_ 







(39) 



In this appendix, we derive expressions for these variations, and we summarize the results. Here we 
repeatedly use the properties, S(x)y = -S(y)x, S(Rx) = RS(x)R T for x, y G M 3 , R G SO(3). 

Variation 5A^.\\: Using (IT~8l> . the variation 5A 1 k X\ is given by 

SA T k Xl = 5F k Xl 

= -F k S{\\)B k 8n k . (40) 

Variation 5C k X\: Since M k+ \ depends on R k +i, SM k+1 x for any i6K 3 can be written as 

5M T k+l x = Af£ +1 (x)C k +i, 

= J^ +x {x)A k C, k +N^ +1 {x)B k 5n k , (41) 

where Af k (x) G IR 3x3 . Using (fT8t and d4TT) . the variation 5C k X k is given by 

<AI = hSF h Ml +1 X 2 k + hF k 5M T k+1 Xl 

= hF k ^ +1 {X 2 k )A k Ck + hF k {-S{Ml+X)®k+^+^l)Bk} Sn k . (42) 

Variation 5BlX\: To obtain the variation 8B k X k , we rewrite the expression of B\X\ as follows. 
Using the definition of B k in d30l . we obtain 

{tr[F fe J d ] / 3x3 - F fc J4 T i3^ = /iF fc A^. 

Since S(x)A T + AS(x) = S({u[A] I 3 x3 - A} T x) for all x G M 3 , A G M 3x3 , the above equation 
can be written in matrix form; 

S{B T k X\)J d FT + F k J d S{B T k X\) = S(hF k X\). (43) 
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Taking a variation of the left hand side of (l43t . 

S{5{B T k \\))J d F^ + F k J d S{8{B T k X\)) + S{B T k X l k )J d S(B k 5n k ) 7 'if + F k S(B k 5n k )J d S(B T k X\) 
= S({tr[F k J d ] J 3x3 - F k J d } T 5{B T k X\)) 
+ S({tr[F fe J d S(£M] J 3x3 - i^S^A*)} F k B k 5n k ). (44) 

Taking a variation of the right hand side of d4*31 >. 

^(/iF fc Ai) = S(hF k S(B k 5n k )Xl + hF k 5X\), 

= S(-hF k S(X l k )B k Sn k + hF k SX l k ). (45) 

Using (EH) and <E2J>, S{B k Xl) is given by 

5(i3^) = 4 T (Al)^ + SM, (46) 

where £ J(-) : M 3 i— > M 3x3 is defined for x G M 3 as 

£%{x) = - {tr[F fe J d ] J 3x3 - F fe J d }- T 

x [{tr^S^x)] / 3x3 - F k J d S{B T k x)} F k B k + hF k S(x)B k ] . 

Variation 6V k X k : The variation 5V k Xl is given by 

bV T k X\ = SF k X 2 k + 6(B T k {-S(F^n k ) + hM T k+1 })Xl 

= hB^ +1 {Xl)A k C k + J^8n k , (47) 

where F k E M 3x3 is defined as 



T T k = -F k S{X k 2 )B k + £l{{S{F^n k ) + hM T k+1 } X\) 

+ B\ {S{Xl\S{Fln k )B k + if) + h^XDB,} . 

Summary: Equations d4*Dt . d4*6T ). d4*2b and d4*71) are expressions of the variations in 091) . Then, CHI 
and (l39l) can be written as 

x fc+ i = A^xfc + A\ 2 SX k , 
SX k = Af +l x k+ i + {A l k+l ) T 5X k+ i, 
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where x k = [Q, <5i7j] T , 5X k = [6X k ' , 5X k 
C-k T^k 



\ and 



Al 1 



A k 2 




-hBB T 



Af 



hF k J^ +1 (Xl)A k -F k S{X\)B k + hF k {S(Ml +1 Xl)B k +J^ +1 (Xl)B k } 
hBT^ +1 (Xt)A k El \X\) + Tl 
In summary, the linear discrete equations of motion can be written as 



Xk+l 




SX k+ i 





Al 1 

, —T 



Al 2 



~~ (Ai+i) Al^Al 1 (^Vi) {I; 



3x3 



421 412^| 
^k+l A k ) 









5X k 



(48) 
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Table 1 

1: Guess an initial multiplier Ao. 

2: Find 77 fc , R k , X\, \\ for k = 1, 2, • • • , JV using the initial conditions and <24l-<28l. 
3: Compute the error in satisfaction of the terminal boundary condition; 

( N = S-^logm^M)),^ = lift - n N . 

Error = ||[Cjv; <577jv][|. 
4: Set Error* = Error, i = 1. 
5: while Error > es. 



6: Find a line search direction; D = 12 . 

7: Setc=l. 

8: while Error* > (1 — 2ac) Error 

9: Choose a trial initial condition Aq = A + cD[£iv; WZjv]. 

10: Find/Tj.,^, Ai, Al for fe = 1, 2, ■ ■ ■ ,7V using the trial initial conditions and 1241 — 1281 . 

1 1 : Compute the error in satisfaction of the terminal boundary condition 

= s-^iogmiRTRj,)), sn% = n%- n N . 

Error* = ||[&;MZ&]||. 
12: Setc = c/2, i = i + 1. 

13: end while 

14: Set Ao = Ao, Error = Error*, (accept the trial) 



15: end while 
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Table 2 



Case 


Model 


J 


logm(i?^ T i?jv) 




(i) 




1.52 


1.77 x 10~ 14 


7.08 x 10" 15 




3D Pendulum 




2.22 x 10~ 16 


2.55 x 10~ 14 


(ii) 




40.22 


(iii) 
(iv) 


Spacecraft 


23.35 
70.74 


2.90 x 10~ 15 
7.31 x 10~ 15 


5.13 x 10~ 15 
1.48 x 10" 14 
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